#include <iostream>
#include <memory>
#include <chrono>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/magnetic_field.hpp>

class ImuSubscriberNode : public rclcpp::Node {
public:
    ImuSubscriberNode() : Node("imu_subscriber") {
        // 声明参数
        this->declare_parameter("imu_topic", "/imu/data");
        this->declare_parameter("mag_topic", "/imu/mag");
        this->declare_parameter("print_interval", 1.0);  // 打印间隔（秒）
        this->declare_parameter("verbose", true);

        // 获取参数
        imu_topic_ = this->get_parameter("imu_topic").as_string();
        mag_topic_ = this->get_parameter("mag_topic").as_string();
        print_interval_ = this->get_parameter("print_interval").as_double();
        verbose_ = this->get_parameter("verbose").as_bool();

        // 创建订阅者
        imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
            imu_topic_, 10, 
            std::bind(&ImuSubscriberNode::imu_callback, this, std::placeholders::_1));

        mag_subscription_ = this->create_subscription<sensor_msgs::msg::MagneticField>(
            mag_topic_, 10, 
            std::bind(&ImuSubscriberNode::mag_callback, this, std::placeholders::_1));

        // 创建定时器用于定期打印统计信息
        auto timer_period = std::chrono::milliseconds(static_cast<int>(print_interval_ * 1000));
        timer_ = this->create_wall_timer(timer_period, 
            std::bind(&ImuSubscriberNode::timer_callback, this));

        RCLCPP_INFO(this->get_logger(), "IMU订阅者已启动");
        RCLCPP_INFO(this->get_logger(), "订阅IMU话题: %s", imu_topic_.c_str());
        RCLCPP_INFO(this->get_logger(), "订阅磁场话题: %s", mag_topic_.c_str());
        RCLCPP_INFO(this->get_logger(), "打印间隔: %.1f 秒", print_interval_);
        RCLCPP_INFO(this->get_logger(), "详细输出: %s", verbose_ ? "开启" : "关闭");

        // 初始化计数器
        imu_msg_count_ = 0;
        mag_msg_count_ = 0;
        last_print_time_ = this->get_clock()->now();
    }

private:
    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
        imu_msg_count_++;
        
        if (verbose_) {
            // 详细输出每条IMU消息
            RCLCPP_INFO(this->get_logger(), 
                "=== IMU数据 #%lu ===", 
                imu_msg_count_);
            
            // 时间戳
            auto time_since_epoch = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
            RCLCPP_INFO(this->get_logger(), 
                "时间戳: %.6f 秒, 坐标系: %s", 
                time_since_epoch, msg->header.frame_id.c_str());

            // 线加速度
            RCLCPP_INFO(this->get_logger(), 
                "自由加速度 [m/s²]: x=%.6f, y=%.6f, z=%.6f",
                msg->linear_acceleration.x, 
                msg->linear_acceleration.y, 
                msg->linear_acceleration.z);


            // 角速度
            RCLCPP_INFO(this->get_logger(), 
                "角速度 [rad/s]: x=%.6f, y=%.6f, z=%.6f",
                msg->angular_velocity.x, 
                msg->angular_velocity.y, 
                msg->angular_velocity.z);

            // 四元数
            RCLCPP_INFO(this->get_logger(), 
                "四元数 [w,x,y,z]: w=%.6f, x=%.6f, y=%.6f, z=%.6f",
                msg->orientation.w, 
                msg->orientation.x, 
                msg->orientation.y, 
                msg->orientation.z);

            // 协方差矩阵（如果有的话）
            bool has_acc_cov = false, has_gyr_cov = false, has_ori_cov = false;
            
            // 检查加速度协方差
            for (int i = 0; i < 9; i++) {
                if (msg->linear_acceleration_covariance[i] != 0.0) {
                    has_acc_cov = true;
                    break;
                }
            }
            
            // 检查角速度协方差
            for (int i = 0; i < 9; i++) {
                if (msg->angular_velocity_covariance[i] != 0.0) {
                    has_gyr_cov = true;
                    break;
                }
            }
            
            // 检查姿态协方差
            for (int i = 0; i < 9; i++) {
                if (msg->orientation_covariance[i] != 0.0) {
                    has_ori_cov = true;
                    break;
                }
            }

            if (has_acc_cov || has_gyr_cov || has_ori_cov) {
                RCLCPP_INFO(this->get_logger(), "协方差信息: 加速度=%s, 角速度=%s, 姿态=%s",
                    has_acc_cov ? "有" : "无",
                    has_gyr_cov ? "有" : "无", 
                    has_ori_cov ? "有" : "无");
            }

            RCLCPP_INFO(this->get_logger(), "========================");
        }
    }

    void mag_callback(const sensor_msgs::msg::MagneticField::SharedPtr msg) {
        mag_msg_count_++;
        
        if (verbose_) {
            // 详细输出每条磁场消息
            RCLCPP_INFO(this->get_logger(), 
                "=== 磁场数据 #%lu ===", 
                mag_msg_count_);
            
            // 时间戳
            auto time_since_epoch = msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9;
            RCLCPP_INFO(this->get_logger(), 
                "时间戳: %.6f 秒, 坐标系: %s", 
                time_since_epoch, msg->header.frame_id.c_str());

            // 磁场强度
            RCLCPP_INFO(this->get_logger(), 
                "磁场强度 [T]: x=%.6e, y=%.6e, z=%.6e",
                msg->magnetic_field.x, 
                msg->magnetic_field.y, 
                msg->magnetic_field.z);

            RCLCPP_INFO(this->get_logger(), "========================");
        }
    }

    void timer_callback() {
        auto now = this->get_clock()->now();
        auto elapsed = now - last_print_time_;
        double elapsed_sec = elapsed.seconds();

        // 计算消息频率
        double imu_freq = imu_msg_count_ / elapsed_sec;
        double mag_freq = mag_msg_count_ / elapsed_sec;

        // 打印统计信息
        RCLCPP_INFO(this->get_logger(), 
            "=== 统计信息 (%.1f秒) ===", 
            elapsed_sec);
        RCLCPP_INFO(this->get_logger(), 
            "IMU消息: %lu条, 频率: %.1f Hz", 
            imu_msg_count_, imu_freq);
        RCLCPP_INFO(this->get_logger(), 
            "磁场消息: %lu条, 频率: %.1f Hz", 
            mag_msg_count_, mag_freq);

        if (imu_msg_count_ > 0) {
            RCLCPP_INFO(this->get_logger(), "IMU数据接收正常");
        } else {
            RCLCPP_WARN(this->get_logger(), "未接收到IMU数据，请检查发布者是否正常运行");
        }

        if (mag_msg_count_ > 0) {
            RCLCPP_INFO(this->get_logger(), "磁场数据接收正常");
        } else {
            RCLCPP_INFO(this->get_logger(), "未接收到磁场数据（可能IMU不支持磁场传感器）");
        }

        RCLCPP_INFO(this->get_logger(), "=========================");

        // 重置计数器
        imu_msg_count_ = 0;
        mag_msg_count_ = 0;
        last_print_time_ = now;
    }

    // 订阅者
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
    rclcpp::Subscription<sensor_msgs::msg::MagneticField>::SharedPtr mag_subscription_;
    rclcpp::TimerBase::SharedPtr timer_;

    // 参数
    std::string imu_topic_;
    std::string mag_topic_;
    double print_interval_;
    bool verbose_;

    // 统计信息
    uint64_t imu_msg_count_;
    uint64_t mag_msg_count_;
    rclcpp::Time last_print_time_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    
    try {
        auto node = std::make_shared<ImuSubscriberNode>();
        rclcpp::spin(node);
    } catch (const std::exception& e) {
        std::cerr << "错误: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }
    
    rclcpp::shutdown();
    return EXIT_SUCCESS;
}
